#ifndef _ROBOTDRIVER_H_
#define _ROBOTDRIVER_H_

#include "PCA9685_servo.h"
#include "SE3Math.h"
#include "robot.h"
#include "trajPlanner.h"

typedef Joints ServoJoint;

void robotdriver_init();
void move_to_home(Joints* joints);
int is_robot_ready(void);
void get_robot_joints(Joints* joints);
void get_robot_joints_list(float* joints_list);
void moveJ(Joints* joints);
void moveL(Pose* pose);
void moveByJoints(Joints* joints_list, int num);
void moveByPoses(Pose* poses_list, int num);

//move slightly
void move_joint1_slightly(int direction);
void move_joint2_slightly(int direction);
void move_joint3_slightly(int direction);
void move_joint4_slightly(int direction);
void move_joint5_slightly(int direction);
void move_joint6_slightly(int direction);

void move_x_slightly(int direction);
void move_y_slightly(int direction);
void move_z_slightly(int direction);

#endif